#ifndef END_FORCE_CONTROL_H
#define END_FORCE_CONTROL_H

#include <stdio.h>
#include <iostream> 
#include <Eigen/Eigen>
#include <math.h> 
//#include other .h 

using namespace std;
using namespace Eigen;

class end_force_control
{
public:	
	//构造和析构函数	
	end_force_control(double * force_sensor_data, double sixth_joint_to_force_sensor);
	/**
     * @param force_sensor_data double[6],用来存储末端六维力传感器的数组
     * @param sixth_joint_to_force_sensor 从机械臂末端坐标系到末端六维力传感器坐标的角度，单位rad
    **/
	~end_force_control(); 

	//public members here;
	//including variables and functions you want to call in scripts;
	bool is_print = false;
	
	void change_sixth_joint_to_force_sensor(double angle);
	/**
     * @brife 改变六轴到六维力传感器坐标系的夹角
     * @param angle 六轴到六维力传感器坐标系的夹角，单位rad
    **/
	
	bool is_balance(double xy_force_limit, double z_force_max, double z_force_min, double xy_torque_limit = 100, double z_torque_limt = 100);
	/**
     * @brife 判断末端是否受力或力矩
     * @param xy_force_limit xy平面上所受力的阈值，单位N
     * @param z_force_max z轴受力阈值上限，单位N
     * @param z_force_min z轴受力阈值下限，单位N
     * @param xy_torque_limit xy轴对应的力矩的阈值，单位Nm
     * @param z_torque_limt z轴对应的力矩的阈值，单位Nm
    **/
		
	void xy_plane_adjust(double * end_position_and_pose, double * end_target_position_and_pose, double force_limit, double adjust_ratio);
	/**
     * @brife 根据六维力传感器调整机械臂末端在xy平面（末端坐标系）中的位置
     * @param end_position_and_pose double[6], 机械臂末端当前位姿，在base坐标系下，单位m, m, m, rad, rad, rad
     * @param end_target_position_and_pose double[6], 机械臂末端目标位姿，在base坐标系下，单位m, m, m, rad, rad, rad
     * @param force_limit 力阈值，超过这个值，机械臂末端位置才会调整，单位N
     * @param adjust_ratio 调整的比例，力超过阈值的值乘以这个比例，得到位移，单位m/N 
    **/
    
    void xy_plane_adjust_with_fixed_step(double * end_position_and_pose, double * end_target_position_and_pose, double force_limit, double adjust_step);
    /**
     * @brife 根据六维力传感器调整机械臂末端在xy平面（末端坐标系）中的位置, 但每次调整的幅度相同，不受力大小的影响
     * @param end_position_and_pose double[6], 机械臂末端当前位姿，在base坐标系下，单位m, m, m, rad, rad, rad
     * @param end_target_position_and_pose double[6], 机械臂末端目标位姿，在base坐标系下，单位m, m, m, rad, rad, rad
     * @param force_limit 力阈值，超过这个值，机械臂末端位置才会调整，单位N
     * @param adjust_step 每次调整的位移，单位m 
    **/
    
    void z_axis_adjust_with_fixed_step(double * end_position_and_pose, double * end_target_position_and_pose, double force_max, double force_min, double adjust_step);
	/**
	 * @brife 根据六维力传感器调整机械臂末端在z轴上的位置, 每次调整的幅度相同，不受力大小的影响
	 * @param end_position_and_pose double[6], 机械臂末端当前位姿，在base坐标系下，单位m, m, m, rad, rad, rad
	 * @param end_target_position_and_pose double[6], 机械臂末端目标位姿，在base坐标系下，单位m, m, m, rad, rad, rad
	 * @param force_max 力最大阈值，高于这个值，机械臂末端位置向外伸，单位N
	 * @param force_min 力最小阈值，低于这个值，机械臂末端位置向里缩，单位N
	 * @param adjust_step 每次调整的位移，单位m 
	**/
    
    bool is_screw_finished(double z_torque_limit);
	/**
	 * @brife 判断是否拧到底
	 * @param z_torque_limit z轴力矩阈值，大于这个值证明拧到底，单位Nm
	**/
	
	bool is_z_move_finished(double z_force_limit);
	/**
	 * @brife 判断是否沿z轴移动到底
	 * @param z_force_limit z轴力阈值，大于这个值证明移动到底，单位N
	**/
    	
protected:
	//protected members here;
	//normally, most of variables and inside functions are here;
	double * force_sensor_data_buff;
	double * end_position_and_pose_buff;
	double * joint_position_buff;
	double sixth_joint_to_force_sensor_buff;
	
	Matrix3d sensor2end_rotate_matrix = Matrix3d::Identity(); 
	
	Matrix3d end2base_rotate_matrix = Matrix3d::Identity();
	
	Matrix3d sensor2base_rotate_matrix = Matrix3d::Identity();
	
	Vector3d base_coordinate = Vector3d::Zero();
	Vector3d sensor_coordinate = Vector3d::Zero();
	
	double force_limit;
	

private:
	//private members here;
};

#endif
